#include "encoder.h"




float EncoderBuffer[4][5] = { 0.f };
float last_value[4] = { 0.f };


int16 Lowpass(int16 X_last, int16 X_new)
{
    int16 new_value, add, count;

    add = (X_new - X_last) * 0.7f;
    new_value = add + X_last;

    return new_value;
}

float GetEncoderData(encoder_index_enum encoder_n)
{
    int index = -1;

    switch (encoder_n)
    {
    case ENCODER1_DIR:
        index = 0;
        break;
    case ENCODER2_DIR:
        index = 1;
        break;
    case ENCODER3_DIR:
        index = 2;
        break;
    case ENCODER4_DIR:
        index = 3;

        break;
    }
    EncoderBuffer[index][0] = EncoderBuffer[index][1];
    EncoderBuffer[index][1] = EncoderBuffer[index][2];
    EncoderBuffer[index][2] = EncoderBuffer[index][3];
    EncoderBuffer[index][3] = EncoderBuffer[index][4];
    EncoderBuffer[index][4] = encoder_get_count(encoder_n);
    encoder_clear_count(encoder_n);
    float res = EncoderBuffer[index][0] * 0.5 + EncoderBuffer[index][1] * 0.5 +
        EncoderBuffer[index][2] * 2 + EncoderBuffer[index][3] * 3 + EncoderBuffer[index][4] * 4;

    //return  EncoderBuffer[index][4];

    return res / 10.f;
}




float GetEncoderSpeed(int encoderdata, float Dt)
{
    return (float) encoderdata / (ENCODER_LINE * Dt);
}


float EncoderSpeedtoEncoderdata(float encoderspeed, float Dt)
{
    return  (encoderspeed * (ENCODER_LINE * Dt));
}